from numpy import matrix, cos, sin, sqrt, pi, linalg
import numpy

# This is a collection of functions used to calculate the effect of the mirrors
# on incident solar radiation (a basically collimated beam). It can be used to 
# calculate the camera error in pixels if given the alt/az errors and a correct 
# scaling factor. It can also do the reverse (which is far more useful). This 
# ability is mainly useful in active correction.

def light_vector(alt, az):
    """Calculate the beam vector of the incoming light whose source is at an
    azimuthal angle az and an altitude alt. North is defined as being the vector 
    along the length of the solar tracker pointing towards mirror 2."""
    
    return matrix([[cos(az)*cos(alt)], [-sin(az)*cos(alt)], [-sin(alt)]])

# nvector stands for normal vector
def mirror1_nvector(alt, az):
    """Calculate the normal vector of the collection mirror given the tracker's 
    current alt and az."""
    return (1/sqrt(2)) * matrix([[sin(az) - cos(az)*cos(alt)], 
                                 [sin(az)*cos(alt) + cos(az)], 
                                 [sin(alt)]])

def mirror1_matrix(alt, az):
    """Calculates the reflection matrix, R, of mirror 1. That is if v_in is the 
    unit column vector of an incident ray of light. Then the reflected ray of 
    light is given by v_out = R * v_in where * is normal matrix multiplication
    """
    
    nvector = (1/sqrt(2)) * matrix([[sin(az) - cos(az)*cos(alt)], 
                                    [sin(az)*cos(alt) + cos(az)],
                                    [sin(alt)]])
    return 2*nvector*nvector.transpose() - numpy.identity(3)

def mirror2_nvector(az):
    return (1/sqrt(2)) * matrix([[-sin(az)],
                                 [-cos(az)],
                                 [-1]])

def mirror2_matrix(az):
    nvector = (1/sqrt(2)) * matrix([[-sin(az)],
                                    [-cos(az)],
                                    [-1]])
    return 2*nvector*nvector.transpose() - numpy.identity(3)

def mirror3_nvector(theta):
    return 1/sqrt(2) * matrix([[-cos(theta) + sin(theta)],
                               [sin(theta) + cos(theta)],
                               [0]])

def grand_mirror_matrix(alt, az):
    
    
    mirror1 = mirror1_matrix(alt, az)
    mirror2 = mirror2_matrix(az)
    mirror3 = matrix([[0,0,1],
                      [0,-1,0],
                      [1,0,0]])
    mirror4 = matrix([[0,-1,0],
                      [-1,0,0],
                      [0,0,-1]]) 
    
    return mirror4*mirror3*mirror2*mirror1

def output_beam(tracker_alt, tracker_az, beam_alt, beam_az):
    
    beam = list()
    mirror = list()
    beam.append(light_vector(beam_alt, beam_az))
    mirror.append(mirror1_nvector(tracker_alt, tracker_az))
    mirror.append(mirror2_nvector(tracker_az))
    mirror.append(1/sqrt(2) * matrix([[1],
                                      [0],
                                      [1]]))
    mirror.append(1/sqrt(2) * matrix([[-1],
                                      [1],
                                      [0]]))
    
    for mirror_vector in mirror:
        beam.append(beam[-1] - 2*float(beam[-1].transpose()*mirror_vector)*
                    mirror_vector)
        
    return beam[4]

def output_beam_2(tracker_alt, tracker_az, beam_alt, beam_az):
    
    beam_in = light_vector(beam_alt, beam_az) 
    beam_out = grand_mirror_matrix(tracker_alt, tracker_az)*beam_in
    
    return beam_out
    
    
def alt_az_err(tracker_alt, tracker_az, x_err, y_err, scaling_factor):
    """Calculate the errors in the current alt and az coordinates and return
    them.
    
    """
        
    err_vec = matrix([[x_err/scaling_factor],
                      [sqrt(1-(x_err/scaling_factor)**2-(y_err/scaling_factor)**2)],
                      [y_err/scaling_factor]]) 
    in_vec = linalg.solve(grand_mirror_matrix(tracker_alt, tracker_az), err_vec)
    true_alt = numpy.arcsin(-float(in_vec[2]))
    
    if float(in_vec[0])/cos(true_alt) > 1:
        true_az = 0.0
    else:
        true_az = numpy.arccos(float(in_vec[0])/cos(true_alt))

    
    alt_err = true_alt - tracker_alt
    az_err = true_az - tracker_az
    
    return alt_err, az_err
   
def x_y_err(tracker_alt, tracker_az, beam_alt, beam_az, scaling_factor):
    
    out_vec = output_beam_2(tracker_alt, tracker_az, beam_alt, beam_az)
    x_err = float(out_vec[0]*scaling_factor)
    y_err = float(out_vec[2]*scaling_factor)
    
    return x_err, y_err
    
    
    
if __name__ == "__main__":
    x_err, y_err = x_y_err(0.2, 0.2, 0.5, 0.5, 100) 
    print x_err, y_err
    alt_err, az_err = alt_az_err(0.2, 0.2, x_err, y_err,100)
    print alt_err, az_err
    
                                 